#include "object3d.h"
#include <cmath>

#include <QDebug>

Object3D::Object3D()
{
}

Point3D* Object3D::addPoint(qreal x, qreal y, qreal z)
{
    Point3D* p;
    foreach(p, m_points) {
        if( ( p->x() == x ) && ( p->y() == y )  && ( p->z() == z ) ) {
            return p;
        }
    }

    p = new Point3D(x,y,z);
    m_points.push_back(p);

    return p;
}

void Object3D::addLine(qreal x1, qreal y1, qreal z1, qreal x2, qreal y2, qreal z2)
{
    Point3D* p1 = addPoint(x1,y1,z1);
    Point3D* p2 = addPoint(x2,y2,z2);

    m_lines.push_back(p1);
    m_lines.push_back(p2);
}

void Object3D::clearObject()
{
    m_lines.clear();
    Point3D* p;
    foreach(p, m_points) {
        delete(p);
    }
    m_points.clear();
}

void Object3D::translate(Point3D &point)
{
    Point3D* p;
    foreach(p, m_points) {
        p->setX( p->x() + point.x() );
        p->setY( p->y() + point.y() );
        p->setZ( p->z() + point.z() );
    }
}

void Object3D::scale(Point3D &pScale, Point3D &pReference)
{
    qreal a = pReference.x();
    qreal b = pReference.y();
    qreal c = pReference.z();
    qreal esc_x = pScale.x();
    qreal esc_y = pScale.y();
    qreal esc_z = pScale.z();

    Point3D* p;
    foreach(p, m_points) {
        p->setX( p->x()*esc_x + a*( 1-esc_x ) );
        p->setY( p->y()*esc_y + b*( 1-esc_y ) );
        p->setZ( p->z()*esc_z + c*( 1-esc_z ) );
    }
}

void Object3D::rotate(qreal &angle, int &axis, Point3D &pReference)
{
    qreal a = pReference.x();
    qreal b = pReference.y();
    qreal c = pReference.z();
    qreal rad = angle*M_PI/180;
    qreal cosT = cos(rad);
    qreal sinT = sin(rad);
    Point3D* p;
    qreal xSenP1, xCosP1, ySenP1, yCosP1, zSenP1, zCosP1;

    switch(axis) {
    case 0: // X
        foreach(p, m_points) {
            qreal y = p->y(); qreal z = p->z();
            p->setY( y*cos(rad) - z*sin(rad) + b*(1-cos(rad)) + c*sin(rad));
            p->setZ( y*sin(rad) + z*cos(rad) + c*(1-cos(rad)) - b*sin(rad));
        }
        qDebug() << getLinesToString() << endl;
        break;

    case 1: // Y        
        foreach(p, m_points) {
            xSenP1 = p->x()*sinT; xCosP1 = p->x()*cosT; zSenP1 = p->z()*sinT; zCosP1 = p->z()*cosT;
            p->setX(  xCosP1 + zSenP1 + a*(1-cosT) - c*sinT );
            p->setZ( -xSenP1 + zCosP1 + c*(1-cosT) + a*sinT );
        }
        break;

    case 2: // Z        
        foreach(p, m_points) {
            xSenP1 = p->x()*sinT; xCosP1 = p->x()*cosT; ySenP1 = p->y()*sinT; yCosP1 = p->y()*cosT;
            p->setX( xCosP1 - ySenP1 + a*(1-cosT) + b*sinT );
            p->setY( xSenP1 + yCosP1 + b*(1-cosT) - a*sinT );
        }
        break;

    default:
        return;
    }
}

void Object3D::rotateAboutArbitraryAxis(qreal &angle, Point3D &p1Line, Point3D &p2Line)
{
    qreal a = p1Line.x(); qreal b = p1Line.y(); qreal c = p1Line.z();
    qreal d = p2Line.x(); qreal e = p2Line.y(); qreal f = p2Line.z();
    qreal u, v, w, u2, v2, w2, x, y, z;
    qreal theta, cosT, sinT;
    qreal L, sL;

    u = d-a;
    v = e-d;
    w = f-c;

    theta = angle*M_PI/180;
    cosT = cos(theta);
    sinT = sin(theta);

    u2 = u*u;
    v2 = v*v;
    w2 = w*w;

    L = u2+v2+w2;
    sL = sqrt(L);

    Point3D* p;
    foreach(p, m_points) {
        x = p->x(); y = p->y(); z = p->z();
        p->setX( ((a*(v2+w2)-u*(b*v+c*w-u*x-v*y-w*z))*(1-cosT) + L*x*cosT + sL*(-c*v+b*w-w*y+v*z)*sinT)/L );
        p->setY( ((b*(u2+w2)-v*(a*u+c*w-u*x-v*y-w*z))*(1-cosT) + L*y*cosT + sL*( c*u-a*w+w*x-u*z)*sinT)/L );
        p->setZ( ((c*(u2+v2)-w*(a*u+b*v-u*x-v*y-w*z))*(1-cosT) + L*z*cosT + sL*(-b*u+a*v-v*x+u*y)*sinT)/L );
    }
}

void Object3D::reflect(int &axis)
{
    Point3D* p;
    switch(axis) {
    case 0: // X
        foreach(p, m_points) {
            p->setY( - p->y() );
            p->setZ( - p->z() );
        }
        break;

    case 1: // Y
        foreach(p, m_points) {
            p->setX( - p->x() );
            p->setZ( - p->z() );
        }
        break;

    case 2:
        foreach(p, m_points) {
            p->setX( - p->x() );
            p->setY( - p->y() );
        }
        break;
    }
}

QList<Point3D*> Object3D::getPoints()
{
    return m_points;
}

QList<Point3D*> Object3D::getLines()
{
    return m_lines;
}

QString Object3D::getLinesToString()
{
    QString r = "";

    for(int i = 0; i < m_lines.size(); i+=2)
        r += QString("(%1,%2,%3) -> (%4,%5,%6)\n").arg(m_lines[i]->x()).arg(m_lines[i]->y()).arg(m_lines[i]->z()).arg(m_lines[i+1]->x()).arg(m_lines[i+1]->y()).arg(m_lines[i+1]->z());

    return r;
}

QString Object3D::getPointsToString()
{
    QString r = "";

    Point3D* p;
    foreach(p, m_points)
        r += QString("(%1, %2, %3), ").arg(p->x()).arg(p->y()).arg(p->z());

    r = r.mid(0,r.length()-2);
    return r;
}
